

#include "foc_controller.h"
#include "utils.h"
#include "main.h"
#include "math.h"
#include "pid.h"
#include "usr_math.h"
#include "arm_math.h"



/* 变换算法相关 */
/*
function: park变换 动态变静态
*/
void park(foc_controller_t *foc_controller, float _sin, float _cos)
{
    foc_controller->id =  foc_controller->ialpha * _cos + foc_controller->ibeta * _sin;
    foc_controller->iq = -foc_controller->ialpha * _sin + foc_controller->ibeta * _cos;
}

/*
function: park逆变换 静态变动态
*/
void revpark(foc_controller_t *foc_controller, float _sin, float _cos)
{
    foc_controller->ualpha = foc_controller->ud * _cos - foc_controller->uq * _sin;
    foc_controller->ubeta  = foc_controller->ud * _sin + foc_controller->uq * _cos;
}


/*
function: clack变换 （32变换）
*/
void clark(foc_controller_t *foc_controller)
{
//    foc_controller->ialpha = foc_controller->ia - 0.5f * foc_controller->ib - 0.5f * foc_controller->ic;
//    foc_controller->ibeta  = 0.8660254f * foc_controller->ib - 0.8660254f * foc_controller->ic;
    foc_controller->ialpha = foc_controller->ia;
    foc_controller->ibeta  = 0.577350269f * foc_controller->ia + 1.15470053838f * foc_controller->ib;
}
/* 变换算法相关 */




void svpwm(foc_controller_t *foc_controller)
{
    float Val_be[2];
    float Vabc[3];
    
    uint8_t n;
    uint8_t A, B, C;
    float T1, T2, T0;
    
    Val_be[0] = foc_controller->ualpha;
    Val_be[1] = foc_controller->ubeta;
    
//    t_al = 0.6667f*t_a - 0.3333f*t_b - 0.3333f*t_c;
//    t_be = 0.57735f*t_b  - 0.57735f*t_c;
    // get n
    A =  Val_be[1] > 0 ? 0 : 4;
    B = (Val_be[1] - 1.732f*Val_be[0]) > 0 ? 0 : 2;
    C = (Val_be[1] + 1.732f*Val_be[0]) > 0 ? 0 : 1;
    
    switch(A+B+C)
    {
        case 0: n = 2; break;
        case 1: n = 3; break;
        case 2: n = 1; break;
        case 3: n = 0; break;
        case 4: n = 0; break;
        case 5: n = 4; break;
        case 6: n = 6; break;
//        case 7: n = 5; break;
        default: n = 5; break;
    }
// 2、时间计算
    float t_al1 = Val_be[0]*0.866f;
    float t_be1 = Val_be[1]*0.500f;
    switch(n)
    {
        case 1: T1 =  t_al1 - t_be1; T2 =  Val_be[1];          break;
        case 2: T1 =  t_al1 + t_be1; T2 = -t_al1 + t_be1;      break;
        case 3: T1 =  Val_be[1];     T2 = -t_al1 - t_be1;      break;
        case 4: T1 = -t_al1 + t_be1; T2 = -Val_be[1];          break;
        case 5: T1 = -t_al1 - t_be1; T2 =  t_al1 - t_be1;      break;
//        case 6: T1 = -Val_be[1];     T2 =  t_al1 + t_be1;      break;
//        default:T1 = 0;              T2 = 0;                   break;
        default: T1 = -Val_be[1];    T2 =  t_al1 + t_be1;     break;
    }
    T0 = 1-T1-T2;
// 3、7段式时间分配
    // 定义过度变量
    float tOdd1, tOdd2, tOdd3, tEven1, tEven2, tEven3;
//    uint16_t vta1000, vtb1000, vtc1000;
    
    tOdd1 = T0*0.500f;
    tOdd2 = T1 + tOdd1;
    tOdd3 = T1 + T2 + tOdd1;
    tEven1 = T2 + tOdd1;
    tEven2 = tOdd1;
    tEven3 = tOdd3;
    switch(n)
    {
        case 1:  Vabc[0] = tOdd1;  Vabc[1] = tOdd2;  Vabc[2] = tOdd3;  break;
        case 2:  Vabc[0] = tEven1; Vabc[1] = tEven2; Vabc[2] = tEven3; break;
        case 3:  Vabc[0] = tOdd3;  Vabc[1] = tOdd1;  Vabc[2] = tOdd2;  break;
        case 4:  Vabc[0] = tEven3; Vabc[1] = tEven1; Vabc[2] = tEven2; break;
        case 5:  Vabc[0] = tOdd2;  Vabc[1] = tOdd3;  Vabc[2] = tOdd1;  break;
        default: Vabc[0] = tEven2; Vabc[1] = tEven3; Vabc[2] = tEven1; break;
    }
    foc_controller->ua = Vabc[0];
    foc_controller->ub = Vabc[1];
    foc_controller->uc = Vabc[2];
}



void foc_run(void)
{
    static cos_sin_t cos_sin;
    usr_cos_sin(sys.foc_controller.ele_rad, &cos_sin);
    
    clark(&sys.foc_controller);
    
    park(&sys.foc_controller, cos_sin.sin, cos_sin.cos);

    sys.foc_controller.uq = pid_clc(&sys.pid_uq, sys.foc_controller.iq, sys.foc_controller.iq_ref);
    sys.foc_controller.ud = pid_clc(&sys.pid_ud, sys.foc_controller.id, sys.foc_controller.id_ref);

    revpark(&sys.foc_controller, cos_sin.sin, cos_sin.cos);
    
    svpwm(&sys.foc_controller);
    
    TIM1->CCR1 = sys.foc_controller.ua * DRV_PWM;
    TIM1->CCR2 = sys.foc_controller.ub * DRV_PWM;
    TIM1->CCR3 = sys.foc_controller.uc * DRV_PWM;
}









